//
// This file is part of cpp-lib, a library of C++ functions and classes.
// Cpp-lib is copyright (C) 2004 and onwards, The CPL Interest Group.
// 
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.
// 
// This library is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
// General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this library (see the file COPYING); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
// 02111-1307  USA
//
// $Id$
//

#include <limits>

#include <cmath>
#include <cassert>

#include "cpp-lib/math-util.h"
#include "cpp-lib/geometry.h"


using namespace cpl::math ;

matrix< double , fixed< 3 , 1 > > const
cpl::math::spherical_to_cartesian(
  double const& theta ,
  double const& phi
) {

  matrix< double , fixed< 3 , 1 > > ret ;
  ret( 1 ) = std::cos( theta ) * std::sin( phi ) ;
  ret( 2 ) = std::sin( theta ) * std::sin( phi ) ;
  ret( 3 ) =                     std::cos( phi ) ;

  return ret ;

}


void cpl::math::cartesian_to_spherical(
  matrix< double , fixed< 3 , 1 > > const& v ,
  double& r                                  ,
  double& theta                              ,
  double& phi
) {

  double const x2 = square( v( 1 ) ) ;
  double const y2 = square( v( 2 ) ) ;
  double const z2 = square( v( 3 ) ) ;

  double const& eps = std::numeric_limits< double >::epsilon() ;

  r = std::sqrt( x2 + y2 + z2 ) ;

  if( r < eps )
  { theta = phi = 0 ; return ; }

  phi = std::acos( v( 3 ) / r ) ;
  assert( phi == phi ) ;

  if( std::fabs( v( 1 ) ) < eps && std::fabs( v( 2 ) ) < eps )
  { theta = 0 ; return ; }

  theta = std::atan2( v( 2 ) , v( 1 ) ) ;
  assert( theta == theta ) ;

}


matrix< double , fixed< 3 , 3 > > const 
cpl::math::sphere_surface_frame
( matrix< double , fixed< 3 , 1 > > const& p ) {

  matrix< double , fixed< 3 , 3 > > C ;

  double const p1s = square( p( 1 ) ) ;
  double const p2s = square( p( 2 ) ) ;
  double const p3s = square( p( 3 ) ) ;

  double const n2s = p1s + p2s ;

  double const n2 = std::sqrt( n2s ) ;

  if( n2 < std::numeric_limits< double >::epsilon() )
  { C( 1 , 1 ) = C( 2 , 2 ) = C( 3 , 3 ) = 1 ; return C ; }

  double const n3s = n2s + p3s ;
  double const n1s = n2s * n3s ;

  double const n1 = std::sqrt( n1s ) ;
  double const n3 = std::sqrt( n3s ) ;

  C( 1 , 1 ) = -p( 1 ) * p( 3 ) / n1 ;
  C( 2 , 1 ) = -p( 2 ) * p( 3 ) / n1 ;
  C( 3 , 1 ) = n2s              / n1 ;
  
  C( 1 , 2 ) = -p( 2 ) / n2 ;
  C( 2 , 2 ) =  p( 1 ) / n2 ;
  C( 3 , 2 ) =  0           ;

  C( 1 , 3 ) = -p( 1 ) / n3 ;
  C( 2 , 3 ) = -p( 2 ) / n3 ;
  C( 3 , 3 ) = -p( 3 ) / n3 ;

  return C ;
 
}
